/**
 * @file control_utils
 * @author yq
 * @brief
 * @version 0.1
 * @date 24-3-18
 *
 * @copyright Copyright (c) 2024
 */
#include "../include/control_utils.h"

// 将角度限制在 ±π 范围内
double normalizeAnglePi(double degrees) {
    return std::fmod(degrees + M_PI, 2.0 * M_PI) - M_PI;
}

double normalizeAngleError(double yaw_error) {
    while (yaw_error > M_PI) {
        yaw_error -= 2 * M_PI;
    }
    while (yaw_error < -M_PI) {
        yaw_error += 2 * M_PI;
    }
    return yaw_error;
}

// 将角度限制在 0 到 π 范围内
double normalizeAngle0To2Pi(double degrees) {
    double normalized_angle = std::fmod(degrees, 2.0 * M_PI);
    if (normalized_angle < 0)
        normalized_angle += 2.0 * M_PI;
    if (normalized_angle > 2 * M_PI)
        normalized_angle -= 2.0 * M_PI;
    return normalized_angle;
}

double radiansToDegrees(double radians) {
    return radians * 180.0 / M_PI;
}

double degreesToRadians(double degrees) {
    return degrees * M_PI / 180;
}

double distanceBetweenPoints(double x1, double y1, double x2, double y2) {
    double dx = x2 - x1;
    double dy = y2 - y1;
    return std::sqrt(dx * dx + dy * dy);
}

void calcTrajectoryCurv(std::shared_ptr<perception_msgs::Trajectory> &ref_trajectory_ptr) {
    double x0, y0, x1, y1, x2, y2;
    double x_fir;
    double x_sec;
    double y_fir;
    double y_sec;
    double kappa;
    for (size_t i = 2; i < ref_trajectory_ptr->trajectoryinfo.trajectorypoints.size(); i++) {
        x0 = ref_trajectory_ptr->trajectoryinfo.trajectorypoints[i - 2].position.x;
        y0 = ref_trajectory_ptr->trajectoryinfo.trajectorypoints[i - 2].position.y;
        x1 = ref_trajectory_ptr->trajectoryinfo.trajectorypoints[i - 1].position.x;
        y1 = ref_trajectory_ptr->trajectoryinfo.trajectorypoints[i - 1].position.y;
        x2 = ref_trajectory_ptr->trajectoryinfo.trajectorypoints[i].position.x;
        y2 = ref_trajectory_ptr->trajectoryinfo.trajectorypoints[i].position.y;

        x_fir = x2 - x1;
        x_sec = (x2 - x1) - (x1 - x0);
        y_fir = y2 - y1;
        y_sec = (y2 - y1) - (y1 - y0);
        kappa = (x_sec * y_fir - x_fir * y_sec) / pow((x_fir * x_fir + y_fir * y_fir), 1.5) + 1e-19;
        ref_trajectory_ptr->trajectoryinfo.trajectorypoints[i].curvature = kappa;
//        ROS_INFO("kappa: %f", kappa);
    }
    if (ref_trajectory_ptr->trajectoryinfo.trajectorypoints.size() >= 3) {
        ref_trajectory_ptr->trajectoryinfo.trajectorypoints[0].curvature =
                ref_trajectory_ptr->trajectoryinfo.trajectorypoints[2].curvature;
        ref_trajectory_ptr->trajectoryinfo.trajectorypoints[1].curvature =
                ref_trajectory_ptr->trajectoryinfo.trajectorypoints[2].curvature;
    }
}


void ExecutiveControl(double acc, ControlData &control_cmd) {
    double throttle = 0.0;
    double brake = 0.0;
    if (acc > 0) {
        throttle = fmax(5, fmin(100, acc / 5 * 100)); // 轿车最大加速度5～10
    } else {
        brake = fmax(5, fmin(-acc / 6 * 100, 100)); // 轿车最大减速度7～8
    }
    control_cmd.throttle = throttle;
    control_cmd.brake = brake;
}


/***********************************find-preview-point**************************************/
perception_msgs::TrajectoryPoint FindPreviewPoint(const double preview_t,
                                                  const CarState cur_pose,
                                                  const std::shared_ptr<perception_msgs::Trajectory> ref_trajectory_ptr,
                                                  const bool is_print_pre_point_info) {
    // 提取trajectoryPoint信息
    const std::vector<perception_msgs::TrajectoryPoint> *trajectory_points_ptr = &(ref_trajectory_ptr->trajectoryinfo.trajectorypoints);

   // 轨迹预瞄点
   perception_msgs::TrajectoryPoint match_point;
   perception_msgs::TrajectoryPoint preview_point;
   bool find_preview_point = false;

   // 根据预瞄时间计算预瞄距离
   float distance = cur_pose.v * preview_t;
   float cur_distance = 0.0;
   float cur_x = 0.0;
   float cur_y = 0.0;


   // 根据当前车辆位置找到MatchPoint
   float match_distance = 1e9;
   int match_Index = 0;
   for (int i = 0; i < trajectory_points_ptr->size(); i++) {
      // std::cout << trajectorypoints[i].position.x << "  " << trajectorypoints[i].position.y << std::endl;
      float distanceTemp = sqrt(pow(cur_pose.x - (*trajectory_points_ptr)[i].position.x, 2) + pow(cur_pose.y - (*trajectory_points_ptr)[i].position.y, 2));
      if (match_distance > distanceTemp) {
         match_distance = distanceTemp;
         match_Index = i;
      }
   }
    match_point = (*trajectory_points_ptr)[match_Index];
    cur_x = (*trajectory_points_ptr)[match_Index].position.x;
    cur_y = (*trajectory_points_ptr)[match_Index].position.y;

    // 根据预瞄距离找到预瞄点
   for (int i = match_Index; i < trajectory_points_ptr->size(); i++) {
        // std::cout << trajectorypoints[i].position.x << "  " << trajectorypoints[i].position.y << std::endl;
        float distanceTemp = sqrt(pow(cur_x - (*trajectory_points_ptr)[i].position.x, 2) + pow(cur_y - (*trajectory_points_ptr)[i].position.y, 2));
        cur_distance += distanceTemp;
        if (cur_distance > distance) {
            preview_point = (*trajectory_points_ptr)[i];
            find_preview_point = true;
            break;
        }
        cur_x = (*trajectory_points_ptr)[i].position.x;
        cur_y = (*trajectory_points_ptr)[i].position.y;
    }

   if (!find_preview_point) {
       preview_point = (*trajectory_points_ptr)[(int)(trajectory_points_ptr->size()) - 1];
       ROS_INFO("没有找到预瞄点！");
   }

   if (is_print_pre_point_info) {
      // 输出基于全局的预瞄点、预瞄距离，轨迹预瞄点
      std::cout << "---------------------------------" << std::endl;
      std::cout << std::fixed << "distance:" << distance << " cur_distance: " <<  cur_distance << std::endl;
      std::cout << "preview_t: " << preview_t << std::endl;
      std::cout << std::fixed << "cur_pose.x:" << cur_pose.x << "   cur_pose.y:" << cur_pose.y << std::endl;
      std::cout << "preview_point.x:" << preview_point.position.x << "   preview_Point.y:" << preview_point.position.y
      <<"   preview_Point.heading:" << preview_point.heading << std::endl;
      std::cout << "---------------------------------" << std::endl;
   }
   return preview_point;
}

/***********************************some-publisher-fuction-rviz**************************************/
// 发布轨迹
void publishTrajectory(const ros::Publisher traj_rviz_pub,
                       const std::shared_ptr<perception_msgs::Trajectory> ref_trajectory_ptr,
                       const perception_msgs::TrajectoryPoint init_point_ref_trajectory_ptr) {
   nav_msgs::Path traj_msg;
   traj_msg.header.frame_id = "base_link";

   for (size_t i = 0; i < ref_trajectory_ptr->trajectoryinfo.trajectorypoints.size(); i++)
   {
      geometry_msgs::PoseStamped position;
      position.header.stamp = ros::Time::now();
      position.header.frame_id = "base_link";
      position.pose.position.x = ref_trajectory_ptr->trajectoryinfo.trajectorypoints[i].position.x - init_point_ref_trajectory_ptr.position.x; // x坐标
      position.pose.position.y = ref_trajectory_ptr->trajectoryinfo.trajectorypoints[i].position.y - init_point_ref_trajectory_ptr.position.y; // y坐标
      position.pose.position.z = 0;                                                                                                                                     // z坐标
      traj_msg.poses.push_back(position);
   }
   traj_rviz_pub.publish(traj_msg);
}
// 发布预测点
void publishPreviewPoint(const ros::Publisher preview_point_rviz_pub,
                         const perception_msgs::TrajectoryPoint preview_point,
                         const perception_msgs::TrajectoryPoint init_point_ref_trajectory_ptr) {
   visualization_msgs::Marker marker;
   marker.header.frame_id = "base_link"; // 坐标系
   marker.header.stamp = ros::Time::now();
   marker.ns = "point_marker";
   marker.id = 0;
   marker.type = visualization_msgs::Marker::SPHERE; // 点形状
   marker.action = visualization_msgs::Marker::ADD;
   marker.pose.position.x = preview_point.position.x - init_point_ref_trajectory_ptr.position.x; // 位置坐标
   marker.pose.position.y = preview_point.position.y - init_point_ref_trajectory_ptr.position.y;
   marker.pose.position.z = 0.0;
   marker.pose.orientation.x = 0.0; // 方向
   marker.pose.orientation.y = 0.0;
   marker.pose.orientation.z = 0.0;
   marker.pose.orientation.w = 1.0;
   marker.scale.x = 0.4; // 尺寸
   marker.scale.y = 0.4;
   marker.scale.z = 0.1;
   marker.color.r = 0.0; // 颜色
   marker.color.g = 0.0;
   marker.color.b = 1.0;
   marker.color.a = 1.0;

   preview_point_rviz_pub.publish(marker);
}
// 发布预测点
void publishPredictPoint(const ros::Publisher predict_point_rviz_pub,
                         const perception_msgs::TrajectoryPoint predict_point,
                         const perception_msgs::TrajectoryPoint init_point_ref_trajectory_ptr) {
   visualization_msgs::Marker marker;
   marker.header.frame_id = "base_link"; // 坐标系
   marker.header.stamp = ros::Time::now();
   marker.ns = "point_marker";
   marker.id = 0;
   marker.type = visualization_msgs::Marker::SPHERE; // 点形状
   marker.action = visualization_msgs::Marker::ADD;
   marker.pose.position.x = predict_point.position.x - init_point_ref_trajectory_ptr.position.x; // 位置坐标
   marker.pose.position.y = predict_point.position.y - init_point_ref_trajectory_ptr.position.y;
   marker.pose.position.z = 0.0;
   marker.pose.orientation.x = 0.0; // 方向
   marker.pose.orientation.y = 0.0;
   marker.pose.orientation.z = 0.0;
   marker.pose.orientation.w = 1.0;
   marker.scale.x = 0.4; // 尺寸
   marker.scale.y = 0.4;
   marker.scale.z = 0.1;
   marker.color.r = 1.0; // 颜色
   marker.color.g = 0.0;
   marker.color.b = 0.0;
   marker.color.a = 1.0;

   predict_point_rviz_pub.publish(marker);
}

// 发布车辆位姿
void publishCurPose(const ros::Publisher cur_pose_rviz_pub,
                    const CarState cur_pose,
                    const perception_msgs::TrajectoryPoint init_point_ref_trajectory_ptr) {
   geometry_msgs::PoseStamped pose_msg;
   pose_msg.header.stamp = ros::Time::now();
   pose_msg.header.frame_id = "base_link"; // 设置坐标系

   // 设置车辆的位置和姿态
   pose_msg.pose.position.x = cur_pose.x - init_point_ref_trajectory_ptr.position.x;
   pose_msg.pose.position.y = cur_pose.y - init_point_ref_trajectory_ptr.position.y;
   pose_msg.pose.position.z = 0.0;
   pose_msg.pose.orientation.x = 0.0;
   pose_msg.pose.orientation.y = 0.0;
   pose_msg.pose.orientation.z = sin(degreesToRadians(cur_pose.yaw) / 2); // 车辆航向角
   pose_msg.pose.orientation.w = cos(degreesToRadians(cur_pose.yaw) / 2);

   cur_pose_rviz_pub.publish(pose_msg); // 发布车辆位姿消息
}

/***********************************output-error-to-file**********************************************/

void outErrorToFile(std::ofstream &outFile_error,
                    const std::shared_ptr<perception_msgs::Trajectory> ref_trajectory_ptr,
                    const CarState cur_pose,
                    const long long duration)
{
    //  提取trajectoryPoint信息
    const std::vector<perception_msgs::TrajectoryPoint> *trajectory_points_ptr = &(ref_trajectory_ptr->trajectoryinfo.trajectorypoints);

    // 根据当前车辆位置找到MatchPoint
    float match_distance = 1e9;
    int match_Index = 0;
    for (int i = 0; i < trajectory_points_ptr->size(); i++)
    {
        float distanceTemp = sqrt(pow(cur_pose.x - (*trajectory_points_ptr)[i].position.x, 2) + pow(cur_pose.y - (*trajectory_points_ptr)[i].position.y, 2));
        if (match_distance > distanceTemp)
        {
            match_distance = distanceTemp;
            match_Index = i;
        }
    }
    perception_msgs::TrajectoryPoint match_point = (*trajectory_points_ptr)[match_Index];

    // 计算速度误差
    double v_error = cur_pose.v - match_point.velocity;
    // 计算加速度误差
    double a_error = cur_pose.acc - match_point.a;
    // 计算偏航角误差
    double yaw_error = radiansToDegrees(normalizeAngleError(degreesToRadians(cur_pose.yaw) - normalizeAngle0To2Pi(match_point.heading)));

    // //计算横向误差
    // double lateral_error;
    // 计算匹配点的切向量（tor）
    double torX = std::cos(match_point.heading);
    double torY = std::sin(match_point.heading);

    // 从匹配点到自我点的向量（d）
    double dX = cur_pose.x - match_point.position.x;
    double dY = cur_pose.y - match_point.position.y;

    // d在tor方向上的投影分量
    double e_s = torX * dX + torY * dY;

    // 计算投影点的x, y坐标
    double px = match_point.position.x + e_s * std::cos(match_point.heading);
    double py = match_point.position.y + e_s * std::sin(match_point.heading);

    double ego_to_project_x = cur_pose.x - px;
    double ego_to_project_y = cur_pose.y - py;
    // 计算横向误差
    double lateral_error = ego_to_project_x * -sin(degreesToRadians(match_point.heading)) +
            ego_to_project_y * cos(degreesToRadians(match_point.heading));

    outFile_error << std::fixed << std::setprecision(6) <<

    v_error << "," << a_error << "," <<
    lateral_error << "," << yaw_error << "," <<
    duration << std::endl;
}